#include "Ransac.h"
#include "dialogs/RansacDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/segmentation/sac_segmentation.h>


#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>

#include <ccPlane.h>

Ransac::Ransac()
	: BaseFilter(FilterDescription("Ransac", "Ransac", "Ransac", ":/toolbar/PclUtils/icons/sac.png", true))
	, m_dialog(0)
	,max_iterations(100)
	,distance_treshold(0.1)
	,find_planes(false)
	,find_lines(false)
	,find_spheres(false)
	,find_cylinders(false)
	,optimize_coefficients(false)
	,probability(0.99)
	,iterate(false)
	,useNormals(false)
	,perpendicular_axis(false)
	,eps_treshold(0.0)
	,iterate_percentage(0)
	,iterate_numofmodels(0)

{


}

Ransac::~Ransac()
{
	if (m_dialog)
		delete m_dialog;
}

int Ransac::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new RansacDialog;
	}

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	//if (cloud)
	//{
	//	ccBBox bBox = cloud->getBB(true,false);
	//	if (bBox.isValid())
	//		m_dialog->radiusDoubleSpinBox->setValue(bBox.getDiagNorm() * 0.005);
	//}
	m_dialog->comboBox_estimator->clear();
	m_dialog->comboBox_estimator->addItem(QString("RANSAC"));
	m_dialog->comboBox_estimator->addItem(QString("LMEDS"));
	m_dialog->comboBox_estimator->addItem(QString("MSAC"));
	return m_dialog->exec() ? 1 : 0;
}

void Ransac::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;

    //fill in parameters from dialog
	estimator = m_dialog->comboBox_estimator->currentText();

	find_planes = m_dialog->radioButton_planes->isChecked();
	find_lines = m_dialog->radioButton_lines->isChecked();
	find_cylinders = m_dialog->radioButton_cylinders->isChecked();
	find_spheres = m_dialog->radioButton_spheres->isChecked();

    distance_treshold = m_dialog->doubleSpinBox_distanceTreshold->value();
	optimize_coefficients = m_dialog->checkBox_optimizeModel->checkState();
	probability = m_dialog->doubleSpinBox_probability->value();
	max_iterations = m_dialog->spinBox_maxIterations->value();

	useNormals = m_dialog->checkBox_useNormals->checkState();
	
	perpendicular_axis = m_dialog->checkBox_perpendicular->checkState();
	axis_x = m_dialog->doubleSpinBox_axisX->value();
	axis_y = m_dialog->doubleSpinBox_axisY->value();
	axis_z = m_dialog->doubleSpinBox_axisZ->value();
	eps_treshold = m_dialog->spinBox_epsTreshold->value()* M_PI / 180;

	iterate = m_dialog->groupBox_iterative->isChecked();
	iterate_percentage = m_dialog->spinBox_percentage->value();
	iterate_numofmodels = m_dialog->spinBox_numModels->value();

}

int Ransac::compute()
{
	ccPointCloud* cc_cloud = getSelectedEntityAsCCPointCloud();

	if(!cc_cloud)
		return -1;

	if(useNormals && !cc_cloud->hasNormals())
		return -14;

	sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);
	sensor_msgs::PointCloud2::Ptr sm_cloud_rest (new sensor_msgs::PointCloud2);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

	cc2smReader converter;
	converter.setInputCloud(cc_cloud);
	converter.getAsSM(*sm_cloud);

	pcl::fromROSMsg(*sm_cloud,*cloud);
	pcl::fromROSMsg(*sm_cloud,*cloud_normals);

	pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);

	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setDistanceThreshold(distance_treshold);
	seg.setOptimizeCoefficients(optimize_coefficients);
	seg.setMaxIterations(max_iterations);
	seg.setProbability(probability);

	//set estimation method
	if(estimator=="RANSAC")
		seg.setMethodType(pcl::SAC_RANSAC);
	if(estimator=="LMEDS")
		seg.setMethodType(pcl::SAC_LMEDS);;
	if(estimator=="MSAC")
		seg.setMethodType(pcl::SAC_MSAC);

	//set model type
	if(find_planes)
		seg.setModelType(pcl::SACMODEL_PLANE);
	if(find_lines)
		seg.setModelType(pcl::SACMODEL_LINE);
	if(find_spheres)
		seg.setModelType(pcl::SACMODEL_SPHERE);
	if(find_cylinders)
		seg.setModelType(pcl::SACMODEL_CYLINDER);

	if(perpendicular_axis)
	{
		seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
		Eigen::Vector3f axis(axis_x,axis_y,axis_z);
		axis.x()=axis_x;axis.y()=axis_y;axis.z()=axis_z;
		seg.setAxis(axis);
		seg.setEpsAngle(eps_treshold);
	}



	sm2ccReader reader;
	ccHObject* cloud_container = new ccHObject();

	int i=0,nr_points = (int) cloud->points.size();
	while( (cloud->points.size() > (iterate_percentage / 100.0) * nr_points) || (i < iterate_numofmodels))
	{
		seg.setInputCloud(cloud);
		seg.segment(*inliers,*coefficients);
		if(inliers->indices.size() == 0)
			break;

		sensor_msgs::PointCloud2::Ptr cloud_plane (new sensor_msgs::PointCloud2);
		pcl::ExtractIndices<sensor_msgs::PointCloud2> extract;
		extract.setInputCloud(sm_cloud);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_plane);

		ccPointCloud* out_c = new ccPointCloud;
		out_c->setName("plane");
		reader.setInputCloud(cloud_plane);
		int good = reader.getAsCC(out_c);
		if(good!=1){
			delete out_c;
			return -1;
		}

		cloud_container->addChild(out_c);

		extract.setNegative(true);
		extract.filter(*sm_cloud_rest);

		sm_cloud=sm_cloud_rest;
		pcl::fromROSMsg(*sm_cloud,*cloud);
		pcl::fromROSMsg(*sm_cloud,*cloud_normals);
		i++;
		if(!iterate)
			break;
	}

	ccPointCloud* rest = new ccPointCloud;
	rest->setName("remaining");
	reader.setInputCloud(sm_cloud);
	int good = reader.getAsCC(rest);
	if(good!=1){
		delete rest;
		return -1;
	}

	cloud_container->addChild(rest);

	emit newEntity(cloud_container);
	return 1;

	//##############################################
	//##### iterative working ######################
	//##############################################
	/*pcl::SACSegmentationFromNormals<pcl::PointXYZ,pcl::Normal> segN;
	segN.setMethodType(pcl::SAC_RANSAC);
	segN.setModelType(pcl::SACMODEL_NORMAL_PLANE);
	segN.setOptimizeCoefficients(optimize_coefficients);
	segN.setDistanceThreshold(distance_treshold);
	segN.setMaxIterations(max_iterations);


	ccHObject* cloud_container = new ccHObject();


	int i=0,nr_points = (int) cloud->points.size();
	while((cloud->points.size() > (m_dialog->spinBox_percentage->value()/100.0) * nr_points
		||  i<m_dialog->spinBox_numModels->value() ))
	{

		segN.setInputCloud(cloud);
		segN.setInputNormals(cloud_normals);
		segN.segment(*inliers,*coefficients);
		if(inliers->indices.size() == 0 )
			break;
		pcl::ExtractIndices<sensor_msgs::PointCloud2> extract;
		extract.setInputCloud(sm_cloud);
		extract.setIndices(inliers);
		extract.setNegative(false);
		sensor_msgs::PointCloud2::Ptr cloud_plane (new sensor_msgs::PointCloud2);
		extract.filter(*cloud_plane);
		
		sm2ccReader reader;
		ccPointCloud* out_c = new ccPointCloud;
		reader.setInputCloud(cloud_plane);
		int good = reader.getAsCC(out_c);
		if(good!=1){
			delete out_c;
			return -1;
		}

		cloud_container->addChild(out_c);

		extract.setNegative(true);
		extract.filter(*sm_cloud_rest);

		sm_cloud=sm_cloud_rest;
		pcl::fromROSMsg(*sm_cloud,*cloud);
		pcl::fromROSMsg(*sm_cloud,*cloud_normals);
		i++;
	}
	emit newEntity(cloud_container);
	return 1;*/

}
